program program_ALS
use madx_ptc_module
use pointer_lattice
implicit none

    interface
       subroutine build_ALS(ALS)
         use madx_ptc_module
         use pointer_lattice
         implicit none
         type(layout), target :: ALS
       end subroutine build_ALS
    end interface

type(layout), pointer:: ALS
type(damapspin) id,m
real(dp) closed_orbit(6),coeff(4),circ,prec,x2,t2,a(6,6),cut
type(probe) ray_closed
type(probe_8) ray
type(internal_state) state
type(normal_spin) normal
integer :: no1=0,np1=0 
type(taylor) average_floquet,average_xp,radii(2),invariant(2)
integer  i,mf,j
logical(lp) mis,tot,exists,newexists
type(vecresonance) diagonal_field
type(taylor), allocatable :: tunes(:),damping(:)
type(taylorresonance) convert
type(fibre),pointer :: p
prec=1.d-6 ! for printing

INQUIRE (FILE = "..\..\..\madgino\alsflat.txt", EXIST = exists)
INQUIRE (FILE = "my_new_flat.txt", EXIST = newexists)


if(exists) then
 Write(6,*)
 Write(6,*) " Hardwired lattice from build_ALS routine-> t "
 Write(6,*)
 Write(6,*) " mad-x flat file from madgino folder -> f? "
 read(5,*) tot
else
 tot=.true.
endif
if(tot) then
 call ptc_ini 
 ALS=>m_u%start 
else
 call ptc_ini_no_append
endif


!Write(6,*) " small misalignments and rotations in lattice ? input t or f "
!read(5,*) mis

MIS=.TRUE.
Write(6,*)
Write(6,*) " small misalignments and rotations in lattice are on "
Write(6,*)

if(tot) then
 call build_ALS(ALS) 
else
 if(newexists) then
    Write(6,*) "reading the new format for flat file "
    CALL  read_lattice_append(M_U,"my_new_flat.txt")
  else
   CALL  READ_AND_APPEND_VIRGIN_general(m_u,"..\..\..\madgino\alsflat.txt")
  endif
 ALS=>m_u%start
endif

if(mis) then
 sig=1.d-5
 cut=4.d0
 call MESS_UP_ALIGNMENT(ALS,SIG,cut)
endif


 state=default0+radiation0+envelope0

 call kanalnummer(mf,file="results.txt")

closed_orbit=0.d0 ! initial guess for closed orbit

call FIND_ORBIT_probe_x(ALS,closed_orbit,state,1.d-5,fibre1=1)

write(6,'(a16,6(1x,g12.5))') " closed orbit = ",closed_orbit(1:6)
write(mf,'(a16,6(1x,g12.5))') " closed orbit = ",closed_orbit(1:6)

 no1=1
  write(6,*) " Order set to 1 for stochastic radiation calculation "
np1=0    !!!  No system  parameters allowed in the stochastic map

call init(state,no1,np1) ! PTC and FPP are properly initialized

call alloc(id,m)  
call alloc(ray)
call alloc(normal)
call alloc(diagonal_field)
 


RAY_CLOSED=closed_orbit

id=1;   ray=RAY_CLOSED+id;   ! ray= closed orbit + identity map

CALL TRACK_PROBE(ALS,RAY,STATE,FIBRE1=1) ! One turn map is computed via the ray



!
!do i=1,6               ! uncomment to artificially increase the damping to 90%!!!
! ray%x(i)=.1d0*ray%x(i)  ! 
!enddo                    ! 

!!! The ray contains a truncated power series algebra
!!! The map we are interested to compute is around the closed orbit: 
!!!  it is really part of a diffential algebra
m=ray   !   The ray is "officially" turned into a "damapspin"  (DA in Berz's honour)
normal = m  ! The map is normalised Orbital 


 ! The normalised vector field is put in phasors' basis
diagonal_field=normal%n%normal%nonlinear

do i=1,c_%nd2
 diagonal_field%sin%v(i)=diagonal_field%sin%v(i)/twopi  
enddo


call print(diagonal_field%cos,mf,prec)

write(mf,*) " "
write(mf,*) "Ordinary Tunes (or imaginary part of vector field)"
write(mf,*) " "
call print(diagonal_field%sin,mf,prec)

allocate(tunes(c_%nd)); allocate(damping(c_%nd))
call alloc(tunes); call alloc(damping);call alloc(convert);

do i=1,c_%nd
 j=2*i
 damping(i)= diagonal_field%cos%v(j).k.j 
 tunes(i)  = diagonal_field%sin%v(j).k.j
enddo

if(c_%ndpt/=0) then
 tunes(3)=diagonal_field%cos%v(6) 
endif


write(mf,*) " "
write(mf,*) " <<<<<<<<<<=======================================>>>>>>>>>>"

write(mf,*) " "
write(mf,*) "Ordinary Tunes extracted from the normalised vector field"

write(mf,*) " "
do i=1,c_%nd
 call print(tunes(i),mf,prec)
enddo

write(mf,*) " "
write(mf,*) "Damping extracted from the normalised vector field"
write(mf,*) " "
do i=1,c_%nd
 call print(damping(i),mf,prec)
enddo

write(mf,*) " "
write(mf,*) "Ordinary Tunes using the old dhdj in Fortran77 (damping absent)"
write(mf,*) " "
write(mf,*) " In real normalised variables  "
do i=1,c_%nd
 call print(normal%n%dhdj%v(i),mf,prec)
enddo

write(mf,*) " Again from dhdj "
write(mf,*) " In phasors variables : should look like the vector field results above  "
do i=c_%nd+1,c_%nd2
 call print(normal%n%dhdj%v(i),mf,prec)
enddo

write(mf,*) " "
write(mf,*) " <<<<<<<<<<=======================================>>>>>>>>>>"

write(mf,*) "           Reproducing the results of dhdj     "
write(mf,*) " "
write(mf,*) "Ordinary Tunes extracted from the normalised vector field"
write(mf,*) " "
do i=1,c_%nd
!!!  In the absence of a complex TPSA,
!!!  "convert" allows conversion between a phasor and cartesian basis 
!!!  without complex concatenation
 convert%sin=0.0_dp 
 convert%cos=tunes(i)
 tunes(i)=convert
 call print(tunes(i),mf,prec)
enddo

write(mf,*) " "
write(mf,*) "Ordinary Tunes as a function of the original phase space"

write(mf,*) " "
do i=1,c_%nd
 tunes(i)= tunes(i)*normal%n%a_t**(-1)
 call print(tunes(i),mf,prec)
enddo


 write(mf,*) "  " 
 write(mf,*) "Equilibrium Beam Sizes " 
 write(mf,*) "  " 
 do i=1,6
  write(mf,'(1X,a3,I1,a3,i1,a4,D18.11)') "<x_",i," x_",i,"> = ",normal%s_ij0(i,i)
 enddo 
 do i=1,6
 do j=i,6 
    if(i/=j) write(mf,'(1X,a3,I1,a3,i1,a4,D18.11)') "<x_",i," x_",j,"> = ",normal%s_ij0(i,j)
 enddo
 enddo 

 write(mf,*) "  " 
 write(mf,*) "Equilibrium Beam Sizes in Phasors Basis " 
 write(mf,*) "  " 
 do i=1,6
 do j=i,6 
  write(mf,'(1X,a3,I1,a3,i1,a4,D18.11,a5,D18.11)') "<h_",i," h_",j,"> = ",real(normal%s_ijr(i,j)),' + i ',aimag(normal%s_ijr(i,j))
 enddo  
 enddo
 


 write(mf,*) "  " 
 write(mf,*) "Equilibrium Chao emittances " 
 write(mf,*) "  " 
 do i=1,3
   write(mf,'(1x,a2,i1,a5,I1,a3,i1,a6,D18.11)') "e_",i,"= <h_",2*i-1," h_",2*i,">/2 = ",  normal%emittance(i)
 enddo

 x2=0.d0
 t2=0.d0
 a=normal%n%a_t
 do i=1,3
  x2=x2+(a(1,2*i-1)**2+a(1,2*i)**2)*normal%emittance(i)
  t2=t2+(a(6,2*i-1)**2+a(6,2*i)**2)*normal%emittance(i)
 enddo

write(6,*)
write(6,*) "<x^2> = ",normal%s_ij0(1,1)," <-- computed with envelopes "
write(6,*) "<x^2> = ",x2," <-- computed with emittances "
write(6,*) "<t^2> = ",normal%s_ij0(6,6)," <-- computed with envelopes "
write(6,*) "<t^2> = ",t2," <-- computed with emittances "

    call print_new_flat(m_u%start,"my_new_flat.txt")
 
write(6,*) "   "
write(6,*) " hit return to terminate program "
write(6,*) "   "
read(5,*)  

close(mf)

end program program_ALS


!=================================================================


subroutine  build_ALS(ALS)
use madx_ptc_module
use pointer_lattice
implicit none

type(layout), target :: ALS

real(dp) :: alpha,lbend, ksd, ksf 
type(fibre)  L1,L2,L3,L4,L5,L6,L7,L8,L9,L10
type(fibre)  L11,L12,L13,L14,L15,L16,L17,L18,L19,L20
type(fibre)  L21,L22,L23,L24,L25,L26,L27,L27A,L27B,L27C,L27D,DS
 type(fibre)  QF1,QF2,QD1,QD2,QFA1,QFA2,sf,sd,cav,bend,vc5,bend1
type(layout) :: sfline,sdline,sup1,supb

!-----------------------------------

call make_states(.true.)
exact_model = .false.
!default = default + nocavity  
call update_states
madlength = .false.


call set_mad(energy = 1.5d0, method = 6, step = 3)

madkind2 = drift_kick_drift


  L1  = drift("L1 ",  2.832695d0)
  L2  = drift("L2 ",  0.45698d0)
  L3  = drift("L3 ",  0.08902d0)
  L4  = drift("L4 ",  0.2155d0)
  L5  = drift("L5 ",  0.219d0)
  L6  = drift("L6 ",  0.107078d0)
  L7  = drift("L7 ",  0.105716d0)
  L8  = drift("L8 ",  0.135904d0)
  L9  = drift("L9 ",  0.2156993d0)
  L10 = drift("L10",  0.089084d0)
   L11= drift("L11",  0.235416d0)
   L12= drift("L12",  0.1245d0)
   L13= drift("L13",  0.511844d0)
   L14= drift("L14",  0.1788541d0)
   L15= drift("L15",  0.1788483d0)
   L16= drift("L16",  0.511849d0)
   L17= drift("L17",  0.1245d0)
   L18= drift("L18",  0.235405d0)
   L19= drift("L19",  0.089095d0)
   L20= drift("L20",  0.2157007d0)
   L21= drift("L21",  0.177716d0)
   L22= drift("L22",  0.170981d0)
   L23= drift("L23",  0.218997d0)
 L24 = drift ("L24",  0.215503d0)
 L25 = drift ("L25",  0.0890187d0)
 L26 = drift ("L26",  0.45698d0)
 L27 = drift ("L27",  2.832696d0)
 L27a  = drift (" L27a",  0.8596d0)
 L27b  = drift (" L27b",  0.1524d0)
 L27c  = drift (" L27c",  0.04445d0)
 L27d  = drift (" L27d",  1.776246d0)
 ds  = drift (" DS  ", 0.1015d0)

  QF1 = QUADRUPOLE(" QF1 ",0.344D0, K1= 2.2474D0+6.447435260914397D-03)
  QF2 = QUADRUPOLE(" QF2 ",0.344D0, K1= 2.2474D0)
  QD1 = QUADRUPOLE(" QD1 ",0.187D0, K1= -2.3368D0-2.593018157427161D-02); 
  QD2 = QUADRUPOLE(" QD2 ",0.187D0, K1= -2.3368D0);  
  QFA1= QUADRUPOLE(" QFA1",0.448D0, K1= 2.8856D0);  
  QFA2= QUADRUPOLE(" QFA2",0.448D0, K1= 2.8856D0);  

!!! 1/2 mad-x value
ksf=(-41.67478927130080d0+0.3392376315938252d0);ksd= (56.36083889436033d0-0.1043679358857811d0);
   sf=sextupole ("sf",2.d0*0.1015d0, K2= ksf);
   sd= sextupole("sd", 2.d0*0.1015d0, K2= ksd);

 VC5=marker("vc5");
ALPHA=0.17453292519943295769236907684886d0;
 
LBEND=0.86621d0;
 
 
BEND = RBEND("BEND", LBEND, ANGLE=ALPHA).q.(-0.778741d0)
BEND1 = RBEND("BEND1", LBEND, ANGLE=ALPHA).q.(-0.778741d0)
 
CAV=RFCAVITY("CAV",L=0.0000d0,VOLT=-1.0d0,REV_FREQ=500.0d6)


sfline=1*sf;
sdline=1*sd;

SUP1=L1+L2+L3+QF1+VC5+L4+L5+QD1+L6+L7+L8+VC5+BEND+VC5+L9+sfline+L10+&
           L11+QFA1+L12+sdline+L13+ &
           L14+BEND+L15+L16+sdline+L17+ &
           QFA2+L18+L19+sfline+L20+BEND+L21+&
           L22+QD2+L23+L24+QF2+L25+ &
           L26+VC5+L27;

SUPb=L1+L2+L3+QF1+VC5+L4+L5+QD1+L6+L7+L8+VC5+BEND+VC5+L9+sfline+L10+&
           L11+QFA1+L12+sdline+L13+ &
           L14+BEND+L15+L16+sdline+L17+ &
           QFA2+L18+L19+sfline+L20+BEND1+L21+&
           L22+QD2+L23+L24+QF2+L25+ &
           L26+VC5+L27;

ALS = 11*sup1+supb+cav;
 
ALS = .ring.ALS

call survey(ALS)


end subroutine build_ALS
